#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>


#include<cv_bridge/cv_bridge.h>
// #include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <image_transport/image_transport.h>


#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>
#include <map>
#include <pcl/filters/passthrough.h>
#include <chrono>
#include <thread>
// #include"depth_rgb_handle/rgb_depth_img.hpp"
#include"depth_rgb_handle/calib.hpp"
#include"depth_rgb_handle/rgb_lidar.hpp"
#include"depth_rgb_handle/pc_handle.hpp"
#include"depth_rgb_handle/utils.hpp"
// #include <X11/Xlib.h>

using namespace std;
using PointType = pcl::PointXYZI;


//rgb lidar handle
RgbLidarHandle rgb_lidar_handle_;

//pc handle
PCHandle pc_handle_;

//gui 
cv::String window_name_ld_ = "lidar";

//kb
string kb_inf_ld_;
// mutex kb_lock_;

//display
ros::Publisher  cloud_pub_, bbox_pub_, pc_plane_pub_, bbox_plane_pub_, pc_plane_back_pub_;

//local map
pcl::PointCloud<PointType>::Ptr local_map_(new pcl::PointCloud<PointType>);
std::deque<pcl::PointCloud<PointType>> cloudQueue_;
// std::deque<double> timeQueue_;
std::mutex local_map_lock_;

void display_th(){
    if(rgb_lidar_handle_.get_is_get_obj()){
        // display_pc(pc_handle_.get_pc(), cloud_pub_);
        display_bbox(pc_handle_.get_global_inf(), bbox_pub_);
        // display_pc_for_measure(pc_handle_.get_global_inf(), pc_plane_pub_, bbox_plane_pub_);
        display_pc_for_measure_one(pc_handle_.get_global_inf(), pc_plane_back_pub_);
    }
}

void get_key_th(){
    kb_inf_ld_ =  string(get_keyboard());  
}

void save_data_th(){
    ObjectPc  object_pc = rgb_lidar_handle_.get_object_points_ld(); 
    // display_pc(object_pc.p_object_pc, cloud_pub_);
    // pc_handle_.get_object_pc(rgb_lidar_handle_.get_object_points_ld());
    pc_handle_.get_object_pc(object_pc);
    // ROS_INFO("rgb_lidar_handle ok");
    pc_handle_.get_bbox_save();
    // pc_handle_.transform_save_pc(pc_handle_.get_local_obj_inf());
    // ROS_INFO("pc_handle_.get_bbox_save ok");
}

void capture_event_ld(int event, int x, int y, int flags, void *params){
    if (event == cv::EVENT_LBUTTONDOWN) {
        rgb_lidar_handle_.record_point(x, y);
    }
    if (event == cv::EVENT_RBUTTONDOWN) {
        lock_guard<mutex> local_map_lock(local_map_lock_);
        local_map_.reset(new pcl::PointCloud<PointType>);
        clear_deque(cloudQueue_);
   }
}

void pc_rgb_CB(const sensor_msgs::PointCloud2ConstPtr& pc_msg, const sensor_msgs::ImageConstPtr& rgb_img_msg){
    // std::chrono::system_clock::time_point t1_, t2_;
    // t1_ = std::chrono::system_clock::now();
    cout<<"cb start"<<endl;
    pcl::PointCloud<PointType>::Ptr pc_temp(new pcl::PointCloud<PointType>);
    // pcl::fromROSMsg(*pc_msg, *pc_temp);

    tf::StampedTransform tf_temp;
    get_tf("/map", "/lidar_link", pc_msg->header.stamp, tf_temp);
    {
        lock_guard<mutex>  local_map_lock(local_map_lock_);
        local_map_.reset(new pcl::PointCloud<PointType>);
        acumu_pc(pc_msg, local_map_, cloudQueue_,  tf_temp);
        pc_temp = handle_local_map(tf_temp, local_map_, true);
    }
    cout<<"local map ok "<<endl;
    pc_temp = cut_pc(pc_temp, 2, 10, -10, 10, -5, 5);
    pcl::VoxelGrid<PointType> dw;
    try{
        dw.setInputCloud(pc_temp);
        dw.setLeafSize(0.09f,0.09f,0.09f);
        dw.filter(*pc_temp);
    }
    catch (pcl::PCLException ex){
        ROS_ERROR("%s", ex.what());
        ROS_ERROR("can not downsample");
        return;
    }
    //debug
    // display_pc(pc_handle_.local_object_pc_.p_object_pc, cloud_pub_);
    // display_pc(pc_temp, cloud_pub_);
    //debug
    // display_pc(pc_temp, cloud_pub_);

    rgb_lidar_handle_.get_pc_img(pc_temp, cv_bridge::toCvCopy(rgb_img_msg, sensor_msgs::image_encodings::BGR8)->image, pc_msg->header.stamp);
    // display_pc(rgb_lidar_handle_.local_pc_, cloud_pub_);
    cv::namedWindow(window_name_ld_);
    cv::moveWindow(window_name_ld_, 10,550);
	cv::setMouseCallback(window_name_ld_, capture_event_ld);
    if(kb_inf_ld_ == "save"){
        save_data_th();
    }
    rgb_lidar_handle_.draw_contours( kb_inf_ld_);
    kb_inf_ld_ = string();
    cout<<"pc handle ok"<<endl;
    cv::Mat img_show ( rgb_lidar_handle_.get_img_show() );
    cout<<"get img show ok"<<endl;
    cv::imshow(window_name_ld_, img_show);
    cout<<"img show ok"<<endl;
    // cv::waitKey(1);
    get_key_th();
    cout<<"get key ok"<<endl;
    display_th();
    // cout<<"display  ok"<<endl;

    // t2_ = std::chrono::system_clock::now();
    // cout<<"cost time   "<<std::chrono::duration_cast<std::chrono::microseconds>( t2_-t1_ ).count()<<std::endl;
    cout<<"CB finish"<<endl;
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "lidar_img");
    ros::NodeHandle nh;
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "/velodyne_points", 5);
    // message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "/points_raw", 5);
    // message_filters::Subscriber<sensor_msgs::Image> rgb_usb_sub(nh, "/rs_top/color/image_raw", 5);
    message_filters::Subscriber<sensor_msgs::Image> rgb_usb_sub(nh, "/camera/color/image_raw", 5);
    // message_filters::Subscriber<sensor_msgs::Image> rgb_usb_sub(nh, "/nexus0/camera/image_raw", 5);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy2;
    message_filters::Synchronizer<MySyncPolicy2> sync2(MySyncPolicy2(20),  pc_sub, rgb_usb_sub);
    sync2.setMaxIntervalDuration(ros::Duration(0.05));
    sync2.registerCallback(boost::bind(&pc_rgb_CB, _1, _2));

    // ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>("/lio_sam/mapping/odometry", 1, odomCB);

    // cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("lidar_img/object_pc",1);
    bbox_pub_=nh.advertise<visualization_msgs::MarkerArray>("lidar_img/bbox", 1);
    // pc_plane_pub_ = nh.advertise<sensor_msgs::PointCloud2>("lidar_img/object_pc_for_measure",1);
    // bbox_plane_pub_=nh.advertise<visualization_msgs::MarkerArray>("lidar_img/bbox_for_measure", 1);
     pc_plane_back_pub_ = nh.advertise<sensor_msgs::PointCloud2>("lidar_img/object_pc_back_for_measure",1);

    ros::spin();

    return 0;
}
